import processing.serial.*;
import controlP5.*;

ControlP5 controlP5;
PFont[] fonts = new PFont[5];

RadioTuner radioTuner;
PIDTuner pidTuner;
RadioMonitor radioMonitor;
MotorMonitor motorMonitor;
ServoMonitor servoMonitor;
GyroMonitor gyroMonitor;
AccelMonitor accelMonitor;
ArtificialHorizon artificialHorizon;
DataGraph dataGraph;
SerPortManager serPortManager;

void setup() {
  size(800,600);  
  controlP5 = new ControlP5(this);
  
  //String[] fontList = PFont.list();
  //println(fontList);
  String fontName = "Arial";
  fonts[0] = createFont(fontName, 8);
  fonts[1] = createFont(fontName, 10);
  fonts[2] = createFont(fontName, 12);
  fonts[3] = createFont(fontName, 16);
  fonts[4] = createFont(fontName, 32);
  
  radioTuner = new RadioTuner(10, 45);
  radioTuner.setup();
  
  pidTuner = new PIDTuner(10, 200);
  pidTuner.setup();
  
  radioMonitor = new RadioMonitor(250, 45);
  radioMonitor.setup();
  
  motorMonitor = new MotorMonitor(620, 120);
  motorMonitor.setup();
  
  servoMonitor = new ServoMonitor(550, 200);
  servoMonitor.setup();
  
  gyroMonitor = new GyroMonitor(400, 280);
  gyroMonitor.setup();
  
  accelMonitor = new AccelMonitor(10, 325);
  accelMonitor.setup();
  
  artificialHorizon = new ArtificialHorizon(240, 380);
  artificialHorizon.setup();
  
  dataGraph = new DataGraph(410, 380, 350, 200);
  dataGraph.setup();
  
  serPortManager = new SerPortManager(this, 300, 5);
  serPortManager.setup();
  serPortManager.fillList();
}

void draw() {
  background(0);
  stroke(255);
  fill(255);
  textFont(fonts[4]); 
  text("Multicopter Utility", 10, 35);
  
  try
  {
    serPortManager.draw();
    
    radioTuner.draw();
    pidTuner.draw();
  
    radioMonitor.update(mouseX * 10, mouseX * 10, mouseX * 10, mouseX * 10, mouseX, mouseX, mouseX, mouseX);
    radioMonitor.draw();
    
    motorMonitor.setMotorCount(mouseX / 80);
    for (int i = 0; i < 8; i++) motorMonitor.setMotorSpeed(i, mouseY * (i + 1));
    motorMonitor.draw();
    
    servoMonitor.setServoCount(mouseX / 80);
    for (int i = 0; i < 8; i++) servoMonitor.setServoPos(i, mouseY * (i + 1));
    servoMonitor.draw();
    
    gyroMonitor.update(((mouseY - 300) / 300.0), ((mouseY - 300) / 300.0), ((mouseY - 300) / 300.0));
    gyroMonitor.draw();
    
    accelMonitor.update(((mouseY - 300) / 150.0), ((mouseY - 300) / 150.0), ((mouseY - 300) / 150.0));
    accelMonitor.draw();
    
    artificialHorizon.update(mouseY - 90, mouseX - 90);
    artificialHorizon.draw();
    
    dataGraph.next();
    for (int i = 0; i < 8; i++) {
      dataGraph.update(i, random(-50, 50));
    }
    dataGraph.draw();
  } catch(Exception ex) {}
}

class RadioTuner {
  int x, y;
  
  RadioTuner(int _x, int _y) {
   x = _x; y = _y; 
  }
  
  Slider sRollScale, sPitchScale, sYawScale, sThrottleScale, sRPExp;
  Button btnCalCenter;
  
  void setup() {
    sRollScale = controlP5.addSlider("", -2, 2, 1, x + 10, y + 25, 100, 10);
    sPitchScale = controlP5.addSlider("", -2, 2, 1, x + 10, y + 40, 100, 10);
    sYawScale = controlP5.addSlider("", -2, 2, 1, x + 10, y + 55, 100, 10);
    sThrottleScale = controlP5.addSlider("", -2, 2, 1, x + 10, y + 70, 100, 10);
    sRPExp = controlP5.addSlider("", 0, 1, 0, x + 10, y + 85, 100, 10);
    btnCalCenter = controlP5.addButton("calibrate center", 1, x + 10, y + 100, 100, 20);
  }
  
  void draw() {
    fill(0);
    stroke(255);
    rect(x, y, 220, 130);
    stroke(255);
    fill(255);
    textFont(fonts[1]); 
    text("Radio Tuning", x + 10, y + 15);
    
    text("Roll Scale", x + 10 + 100 + 5, y + 35);
    text("Pitch Scale", x + 10 + 100 + 5, y + 50);
    text("Yaw Scale", x + 10 + 100 + 5, y + 65);
    text("Throttle Scale", x + 10 + 100 + 5, y + 80);
    text("Roll/Pitch Exponent", x + 10 + 100 + 5, y + 95);
    
    if (mouseX > x + 10 && mouseX < x + 10 + 110 && mouseY > y + 85 && mouseY < y + 85 + 10) {
      fill(0);
      stroke(255);
      rect(100 + x + 10, y + 100, 100, -100);
      for (int i = 0; i < 99; i++) {
        float expScaler= 100.0 * exp(sRPExp.value()) / 100.0;
        float ypt1 = i * exp(i * sRPExp.value() / 100.0) / expScaler;
        float ypt2 = (i + 1) * exp((i + 1) * sRPExp.value() / 100.0) / expScaler;
        line(110 + x + i, y + 100 - ypt1, 110 + x + i + 1, y + 100 - ypt2);
      }
    }
  }
}

class RadioMonitor {
  int x, y;
  
  RadioMonitor(int _x, int _y) {
   x = _x; y = _y; 
  }
  
  float roll, pitch, throttle, yaw, aux1, aux2, aux3, aux4;
  float scaler = 1.0/20.0, dia = 6;  
  
  void update(float _roll, float _pitch, float _throttle, float _yaw, float _aux1, float _aux2, float _aux3, float _aux4) {
    roll = _roll;
    pitch = _pitch;
    throttle = _throttle;
    yaw = _yaw;
    aux1 = _aux1;
    aux2 = _aux2;
    aux3 = _aux3;
    aux4 = _aux4;
  }
  
  void setup() {
  }
  
  void draw() {
    fill(0);
    stroke(255);
    rect(x, y, 300, 140);
    stroke(255);
    fill(255);
    textFont(fonts[1]); 
    text("Radio Monitor", x + 10, y + 15);
    
    stroke(255);
    fill(0);
    rect(x + 10, y + 25, 100, 100);
    rect(x + 10 + 110, y + 25, 100, 100);
    
    stroke(255, 0, 0);
    line(x + 10, y + 25 + 50, x + 10 + 100, y + 25 + 50);
    line(x + 10 + 50, y + 25, x + 10 + 50, y + 25 + 100);
    line(x + 10 + 110, y + 25 + 50, x + 10 + 110 + 100, y + 25 + 50);
    line(x + 10 + 110 + 50, y + 25, x + 10 + 110 + 50, y + 25 + 100);
    
    noStroke();
    fill(0, 255, 0);
    ellipse(x + 10 + 50 + (roll - 1500) * scaler, y + 25 + 50 + (pitch - 1500) * scaler, dia, dia);
    ellipse(x + 10 + 110 + 50 + (yaw - 1500) * scaler, y + 25 + 50 + (throttle - 1500) * scaler, dia, dia);
    
    stroke(255);
    fill(255);
    textFont(fonts[1]); 
    int tX, tY;
    
    if ((roll - 1500) * scaler > 0) {
      tX = x + 10 + 50 + round((roll - 1500) * scaler) - 50;
    } else {
      tX = x + 10 + 50 + round((roll - 1500) * scaler) + 5;
    }
    if ((pitch - 1500) * scaler > 0) {
      tY = y + 25 + 50 + round((pitch - 1500) * scaler) - 15;
    } else {
      tY = y + 25 + 50 + round((pitch - 1500) * scaler) + 7;
    }

    text("roll: " + round(roll), tX, tY);
    text("pitch: " + round(pitch), tX, tY + 10);
    
    if ((yaw - 1500) * scaler > 0) {
      tX = x + 10 + 110 + 50 + round((yaw - 1500) * scaler) - 50;
    } else {
      tX = x + 10 + 110 + 50 + round((yaw - 1500) * scaler) + 5;
    }
    if ((throttle - 1500) * scaler > 0) {
      tY = y + 25 + 50 + round((throttle - 1500) * scaler) - 15;
    } else {
      tY = y + 25 + 50 + round((throttle - 1500) * scaler) + 7;
    }
    
    text("yaw: " + round(yaw), tX, tY);
    text("throttle: " + round(throttle), tX, tY + 12);
    
    text("aux 1:", x + 230, y + 30);
    text(round(aux1), x + 230, y + 45 - (4 * 1));
    text("aux 2:", x + 230, y + 70- (4 * 2));
    text(round(aux2), x + 230, y + 85- (4 * 3));
    text("aux 3:", x + 230, y + 105- (4 * 4));
    text(round(aux3), x + 230, y + 120- (4 * 5));
    text("aux 4:", x + 230, y + 140- (4 * 6));
    text(round(aux4), x + 230, y + 155- (4 * 7));
  }
}

class MotorMonitor {
  int x, y;
  
  MotorMonitor(int _x, int _y) {
   x = _x; y = _y; 
  }
  
  float scaler = 50.0/2000.0;
  
  int motorCount = 4;
  void setMotorCount(int c) {
    motorCount = c;
  }
  
  float rotation = -1.0;
  void setRotation(float x) {
    rotation = x;
  }
  
  float[] motorSpeed = new float[8];
  void setMotorSpeed(int i, float speed) {
    motorSpeed[i] = speed;
  }
  
  void setup() {
  }
  
  void draw() {
    fill(0);
    stroke(255);
    
    rect(x - 60, y - 70, 200, 130);
    
    fill(255);
    textFont(fonts[1]); 
    text("Motor Monitor", x - 50, y - 55);

    fill(0);
    
    ellipse(x, y, 100, 100);
    
    stroke(255, 0, 0);
    
    float angleStep = TWO_PI / motorCount;
    
    for (int i = 0; i < motorCount; i++) {
      int startX, startY, endX, endY;
      endX = round(cos(rotation + angleStep * i - (TWO_PI / 4.0)) * (motorSpeed[i] * scaler));
      endY = round(sin(rotation + angleStep * i - (TWO_PI / 4.0)) * (motorSpeed[i] * scaler));
      for (int j = -1; j <= 1; j++)
      {
        startX = round(cos(angleStep * i) * j);
        startY = round(sin(angleStep * i) * j);
        line(x + startX, y + startY, x + endX, y + endY);
      }
    }
    
    fill(255);
    stroke(255);
    for (int i = 0; i < motorCount; i++) {
      text("motor " + i + ": " + round(motorSpeed[i]), x + 55, y - 50 + i * 13);
    }
  }
}

class ServoMonitor {
  int x, y;
  
  ServoMonitor(int _x, int _y) {
   x = _x; y = _y; 
  }
  
  float scaler = 50.0/2000.0;
  
  int servoCount = 0;
  void setServoCount(int c) {
    servoCount = c;
  }
  
  float[] servoPos = new float[8];
  void setServoPos(int i, float pos) {
    servoPos[i] = pos;
  }
  
  void setup() {
  }
  
  void draw() {
    
    stroke(255);
    noFill();
    rect(x, y - 3, 185, 25 + 15 * servoCount);
    fill(255);
    textFont(fonts[1]); 
    text("Servo Monitor", x + 10, y + 10);

    for (int i = 0; i < servoCount; i++) {
      stroke(255);
      fill(255);
      text("S" + i + ":", x + 10, y + 25 + (15 * i));
      stroke(255);
      fill(64, 0, 0);
      rect(x + 40, y + 15 + (15 * i), 100, 10);
      fill(255, 0, 0);
      rect(x + 40, y + 15 + (15 * i), round(servoPos[i] * scaler), 10);
      fill(255);
      text(round(servoPos[i]), x + 145, y + 25 + (15 * i));
    }    
  }
}

class PIDTuner {
  int x, y;
  
  PIDTuner(int _x, int _y) {
   x = _x; y = _y; 
  }
  
  int rowSpace = 15;
  int colSpace = 50;
  
  Numberbox numRollP, numRollI, numRollD, numPitchP, numPitchI, numPitchD, numYawP, numYawI, numYawD;
  Numberbox numLevelP, numLevelI, numRollPitchRate, numYawRate, numThrotAttenu;  
  
  void setup() {
    numRollP = controlP5.addNumberbox("", 0, x + colSpace * 1, y + rowSpace * 1, 40, 13);
    numRollP.setDirection(Controller.HORIZONTAL); numRollP.setMin(0); numRollP.setMax(20); numRollP.setDecimalPrecision(1); numRollP.setMultiplier(0.1);
    numPitchP = controlP5.addNumberbox("", 0, x + colSpace * 1, y + rowSpace * 2, 40, 13);
    numPitchP.setDirection(Controller.HORIZONTAL); numPitchP.setMin(0); numPitchP.setMax(20); numPitchP.setDecimalPrecision(1); numPitchP.setMultiplier(0.1);
    numYawP = controlP5.addNumberbox("", 0, x + colSpace * 1, y + rowSpace * 3, 40, 13);
    numYawP.setDirection(Controller.HORIZONTAL); numYawP.setMin(0); numYawP.setMax(20); numYawP.setDecimalPrecision(1); numYawP.setMultiplier(0.1);
    
    numRollI = controlP5.addNumberbox("", 0, x + colSpace * 2, y + rowSpace * 1, 40, 13);
    numRollI.setDirection(Controller.HORIZONTAL); numRollI.setMin(0); numRollI.setMax(0.250); numRollI.setDecimalPrecision(3); numRollI.setMultiplier(0.001);
    numPitchI = controlP5.addNumberbox("", 0, x + colSpace * 2, y + rowSpace * 2, 40, 13);
    numPitchI.setDirection(Controller.HORIZONTAL); numPitchI.setMin(0); numPitchI.setMax(0.250); numPitchI.setDecimalPrecision(3); numPitchI.setMultiplier(0.001);
    numYawI = controlP5.addNumberbox("", 0, x + colSpace * 2, y + rowSpace * 3, 40, 13);
    numYawI.setDirection(Controller.HORIZONTAL); numYawI.setMin(0); numYawI.setMax(0.250); numYawI.setDecimalPrecision(3); numYawI.setMultiplier(0.001);
    
    numRollD = controlP5.addNumberbox("", 0, x + colSpace * 3, y + rowSpace * 1, 40, 13);
    numRollD.setDirection(Controller.HORIZONTAL); numRollD.setMin(0); numRollD.setMax(50); numRollD.setDecimalPrecision(0); numRollD.setMultiplier(1);
    numPitchD = controlP5.addNumberbox("", 0, x + colSpace * 3, y + rowSpace * 2, 40, 13);
    numPitchD.setDirection(Controller.HORIZONTAL); numPitchD.setMin(0); numPitchD.setMax(50); numPitchD.setDecimalPrecision(0); numPitchD.setMultiplier(1);
    numYawD = controlP5.addNumberbox("", 0, x + colSpace * 3, y + rowSpace * 3, 40, 13);
    numYawD.setDirection(Controller.HORIZONTAL); numYawD.setMin(0); numYawD.setMax(50); numYawD.setDecimalPrecision(0); numYawD.setMultiplier(1);
    
    numLevelP = controlP5.addNumberbox("", 0, x + colSpace * 1, y + rowSpace * 4, 40, 13);
    numLevelP.setDirection(Controller.HORIZONTAL); numLevelP.setMin(0); numLevelP.setMax(25); numLevelP.setDecimalPrecision(1); numLevelP.setMultiplier(0.1);
    numLevelI = controlP5.addNumberbox("", 0, x + colSpace * 2, y + rowSpace * 4, 40, 13);
    numLevelI.setDirection(Controller.HORIZONTAL); numLevelP.setMin(0); numLevelP.setMax(0.250); numLevelP.setDecimalPrecision(3); numLevelP.setMultiplier(0.001);
    
    numRollPitchRate = controlP5.addNumberbox("", 0, x + colSpace * 4, round(y + rowSpace * 1.5), 40, 13);
    numRollPitchRate.setDirection(Controller.HORIZONTAL); numRollPitchRate.setMin(0); numRollPitchRate.setMax(1); numRollPitchRate.setDecimalPrecision(2); numRollPitchRate.setMultiplier(0.01);
    
    numYawRate = controlP5.addNumberbox("", 0, x + colSpace * 4, round(y + rowSpace * 3), 40, 13);
    numYawRate.setDirection(Controller.HORIZONTAL); numYawRate.setMin(0); numYawRate.setMax(1); numYawRate.setDecimalPrecision(2); numYawRate.setMultiplier(0.01);    
    
    numThrotAttenu = controlP5.addNumberbox("", 0, round(x + colSpace * 1.5), round(y + rowSpace * 5.7), 40, 13);
    numThrotAttenu.setDirection(Controller.HORIZONTAL); numThrotAttenu.setMin(0); numThrotAttenu.setMax(1); numThrotAttenu.setDecimalPrecision(2); numThrotAttenu.setMultiplier(0.01);    
  }
  
  void draw() {
    stroke(255);
    noFill();
    rect(x - 5, y - 17, 255, 135);
    fill(255);
    textFont(fonts[1]); 
    text("PID Tuning", x, y - 5);
    text("P", 5 + x + colSpace * 1, 10 + y + rowSpace * 0);
    text("I", 5 + x + colSpace * 2, 10 + y + rowSpace * 0);
    text("D", 5 + x + colSpace * 3, 10 + y + rowSpace * 0);
    text("Rate", x + colSpace * 4, 10 + y + rowSpace * 0);
    text("Roll", x + colSpace * 0, 10 + y + rowSpace * 1);
    text("Pitch", x + colSpace * 0, 10 + y + rowSpace * 2);
    text("Yaw", x + colSpace * 0, 10 + y + rowSpace * 3);
    text("Level", x + colSpace * 0, 10 + y + rowSpace * 4);    
    text("Throttle PID", x + colSpace * 0, round(15 + y + rowSpace * 5.25));
    text("Attenuation", x + colSpace * 0, round(15 + y + rowSpace * 6));
    stroke(64);
    line(x, round(5 + y + rowSpace * 5), x + 245, round(5 + y + rowSpace * 5));
  }
}

class GyroMonitor {
  int x, y;
  
  GyroMonitor(int _x, int _y) {
    x = _x; y = _y; 
  }
  
  float gyroRoll, gyroPitch, gyroYaw;
  
  void update(float _gyroRoll, float _gyroPitch, float _gyroYaw) {
    gyroRoll = _gyroRoll;
    gyroPitch = _gyroPitch;
    gyroYaw = _gyroYaw;
  }
  
  float yawGyroScale = 1.0;
  float rollPitchGyroScale = 1.0;
  
  void setup() {
  }
  
  void draw() {
    stroke(255);
    fill(255);
    textFont(fonts[1]); 
    text("Gyro Sensors", x - 75, y - 70);
    noFill();
    
    rect(x - 80, y - 85, 160, 140);
    
    for (int i = -2; i <= 2; i++) {
      arc(x, y, 120 + i, 120 + i, (-TWO_PI / 2.0), 0);
    }
    fill(0, 255, 0);
    stroke(0, 255, 0);
    int xPt = x + round(60 * cos(TWO_PI * gyroYaw * yawGyroScale - (TWO_PI / 4.0)));
    int yPt = y + round(60 * sin(TWO_PI * gyroYaw * yawGyroScale - (TWO_PI / 4.0)));
    ellipse(xPt, yPt, 6, 6);
    if (xPt < x) xPt -= 75;
    fill(255);
    stroke(255);
    text("Yaw: " + round(gyroYaw * 10) / 10.0, x, y - 68);
    //text("Gyros", x - 13, y - 17);
    text("Pitch", x + 12, y - 7);
    text(round(gyroPitch * 10) / 10.0, x + 2, y + 7);
    text("Roll", x - 33, y - 7);
    text(round(gyroRoll * 10) / 10.0, x - 33, y + 7);
    
    noFill();
    for (int i = -2; i <= 2; i++) {
      arc(x, y, 100 + i, 100 + i, (-TWO_PI / 5.0), (TWO_PI / 5.0));
      arc(x, y, 100 + i, 100 + i, (TWO_PI * 2.0 / 4.0) - (TWO_PI / 5.0), (TWO_PI * 2.0 / 4.0) + (TWO_PI / 5.0));
    }
    
    fill(0, 255, 0);
    stroke(0, 255, 0);
    xPt = x + round(50 * cos(TWO_PI * gyroPitch * rollPitchGyroScale));
    yPt = y + round(50 * sin(TWO_PI * gyroPitch * rollPitchGyroScale));
    ellipse(xPt, yPt, 6, 6);
    xPt = x - round(50 * cos(TWO_PI * gyroRoll * rollPitchGyroScale));
    yPt = y + round(50 * sin(TWO_PI * gyroRoll * rollPitchGyroScale));
    ellipse(xPt, yPt, 6, 6);
    
    stroke(255, 0, 0);
    line(x - 45, y, x - 55, y);
    line(x + 45, y, x + 55, y);
    line(x, y - 55, x, y - 65);
  }
}

class AccelMonitor {
  int x, y;
  
  AccelMonitor(int _x, int _y) {
    x = _x; y = _y; 
  }
  
  float accX, accY, accZ;
  
  void update(float _accX, float _accY, float _accZ) {
    accX = _accX;
    accY = _accY;
    accZ = _accZ;
  }
  
  float scaler = 30.0;
  float dia = 6;
  
  void setup() {
  }
  
  void draw() {
    fill(0);
    stroke(255);
    rect(x, y, 170, 140);
    stroke(255);
    fill(255);
    textFont(fonts[1]); 
    text("Accelerometer Monitor", x + 10, y + 15);
    
    stroke(255);
    fill(0);
    rect(x + 10, y + 25, 100, 100);
    rect(x + 10 + 110, y + 25, 10, 100);
    
    stroke(255, 0, 0);
    line(x + 10, y + 25 + 50, x + 10 + 120, y + 25 + 50);
    line(x + 10 + 50, y + 25, x + 10 + 50, y + 25 + 100);
    
    noStroke();
    fill(0, 255, 0);
    ellipse(x + 10 + 50 + accX * scaler, y + 25 + 50 + accY * scaler, dia, dia);
    ellipse(x + 10 + 115, y + 25 + 50 + accZ * scaler, dia, dia);
    
    stroke(255);
    fill(255);
    textFont(fonts[1]); 
    int tX, tY;
    
    if (accX * scaler > 0) {
      tX = x + 10 + 50 + round(accX * scaler) - 35;
    } else {
      tX = x + 10 + 50 + round(accX * scaler) + 5;
    }
    if (accY * scaler > 0) {
      tY = y + 25 + 50 + round(accY * scaler) - 15;
    } else {
      tY = y + 25 + 50 + round(accY * scaler) + 7;
    }

    text("X: " + round(accX * 10.0) / 10.0, tX, tY);
    text("Y: " + round(accY * 10.0) / 10.0, tX, tY + 10);
    text("Z: " + round(accZ * 10.0) / 10.0, x + 10 + 125, y + 30 + 50 + accZ * scaler);
  }
}

class ArtificialHorizon {
  int x, y;
  
  ArtificialHorizon(int _x, int _y) {
    x = _x; y = _y; 
  }
  
  float dia = 100.0;
  
  float roll, pitch;
  
  void update(float _roll, float _pitch) {
    roll = _roll;
    pitch = _pitch;
  }
  
  void setup() {
  }
  
  void draw() {
    
    fill(0);
    stroke(255);
    rect(x - 55, y - 65, 110, 150);
    
    ellipse(x, y, dia, dia);

    float x1 = (dia / 2.0) * cos(radians(roll));
    float y1 = (dia / 2.0) * sin(radians(roll));
    float x2 = (dia / 2.0) * cos(radians(roll + 180.0));
    float y2 = (dia / 2.0) * sin(radians(roll + 180.0));
    float x3 = (dia * sin(radians(pitch)) / 2.0) * cos(radians(roll - 90.0));
    float y3 = (dia * sin(radians(pitch)) / 2.0) * sin(radians(roll - 90.0));
    
    fill(128);
    
    triangle(x + x1, y + y1, x + x2, y + y2, x + x3, y + y3);
    
    fill(255);
    
    textFont(fonts[1]);
    text("Artificial Horizon", x - 50, y - 55);
    text("Roll: " + round(roll), x - 50, y + 60);
    text("Pitch: " + round(pitch), x - 50, y + 75);
    
    stroke(255, 0, 0);
    line(x - dia / 2, y, x + dia / 2, y);
  }
}

class DataGraph {
  int x, y, w, h;
  
  DataGraph(int _x, int _y, int _w, int _h) {
    x = _x; y = _y; w = _w; h = _h;
    data = new float[8][];
    filled = new boolean[8][];
    for (int i = 0; i < 8; i++) {
      data[i] = new float[_w];
      filled[i] = new boolean[_w];
      for (int j = 0; j < _w; j++) {
        filled[i][j] = false;
      }
    }
    
    label = new String[8];
    label[0] = "Roll Gyro";
    label[1] = "Pitch Gyro";
    label[2] = "Yaw Gyro";
    label[3] = "X Accel";
    label[4] = "Y Accel";
    label[5] = "Z Accel";
    label[6] = "Mag";
    label[7] = "Baro";
    
    enabled = new boolean[] { true, true, true, true, true, true, false, false };
    colors = new color[8];
    colors[0] = color(255, 0, 0);
    colors[1] = color(0, 255, 0);
    colors[2] = color(0, 0, 255);
    colors[3] = color(255, 255, 0);
    colors[4] = color(255, 0, 255);
    colors[5] = color(0, 255, 255);
    colors[6] = color(255, 128, 64);
    colors[7] = color(128, 64, 255);
    
    prevVal = new float[8];
  }
  
  float[][] data;
  String[] label;
  boolean[] enabled;
  boolean[][] filled;
  color[] colors;
  float[] prevVal;
  int ptr = 0;
  
  void update(int idx, float y) {
    data[idx][ptr] = y;
    filled[idx][ptr] = true;
  }
  
  void next() {
    ptr = (ptr + 1) % w;
    for (int i = 0; i < 8; i++) {
      filled[i][ptr] = false;
    }
  }
  
  boolean mouseFlag = false;
  
  void setup() {
  }
  
  void draw() {
    
    int centerLine = y + h / 2;
    
    fill(0);
    stroke(255);
    rect(x - 110, y - 20, w + 120, h + 30);
    fill(255);
    textFont(fonts[1]);
    text("Data Graph", x - 100, y - 5);
    noFill();
    rect(x, y, w, h);
    
    for (int i = 0; i < 8; i++) {
      prevVal[i] = data[i][ptr];
    }
    int idx = ptr;
    int xPos = 0;
    do {
      
      for (int i = 0; i < 8; i++) {
        if (enabled[i] && filled[i][idx]) {
          stroke(colors[i]);
          line(x + xPos, centerLine + prevVal[i], x + xPos + 1, centerLine + data[i][idx]);
          prevVal[i] = data[i][idx];
        }
      }
      
      xPos++;
      idx--;
      if (idx <= -1) {
        idx = w - 1;
      }
    }
    while (xPos < w);
    
    if (mousePressed == false) {
      mouseFlag = false;
    }
    
    for (int i = 0; i < 8; i++) {
      fill(colors[i]);
      stroke(255);
      rect(x - 100, y + (17 * i), 13, 13);
      if (mouseX > x - 100 && mouseX < x && mouseY > y + (17 * i) && mouseY < y + (17 * (i + 1))) {
        fill(64);
        if (mousePressed) {
          if (mouseFlag == false) {
            enabled[i] = !enabled[i];
            mouseFlag = true;
          }
        }
      }
      else {
        fill(0);
      }      
      rect(x - 100 + 13, y + (17 * i), 80, 13);
      fill(255);
      text(label[i], x - 100 + 15, y + 11 + (17 * i));
      if (enabled[i] == false) {
        fill(0);
        noStroke();
        rect(x - 100 + 2, y + (17 * i) + 2, 13 - 4, 13 - 4);
      }
    }
    stroke(255);
    line(x, centerLine, x + w, centerLine);
  }
}

class SerPortManager {
  PApplet parent;
  int x, y;
  SerPortManager(PApplet _parent, int _x, int _y) {
    x = _x; y = _y;
    parent = _parent;
  }
  
  DropdownList portList;
  Serial port;
  
  void setup() {
    portList = controlP5.addDropdownList("portList", x + 50, y + 18, 100, 20);
  }
  
  void fillList() {
    portList.clear();
    for(int i = 0; i < Serial.list().length; i++)
      portList.addItem(shortifyPortName(Serial.list()[i], 13), i);
  }
  
  String shortifyPortName(String portName, int maxlen) {
    String shortName = portName;
    if(shortName.startsWith("/dev/"))
      shortName = shortName.substring(5);  
      
    if(portName.length() > maxlen) {
      shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2));
    }
    return shortName;
  }
  
  void initPort(int portNum) {
    try {
      String portPos = Serial.list()[portNum];
      port = new Serial(parent, portPos, 115200);
      msg = "\"" + shortifyPortName(portPos, 8) + "\" is open";
    }
    catch (Exception ex) {
      msg = "Error: " + ex.getMessage();
    }
  }
  
  String msg;
  
  void draw() {
    fill(0);
    stroke(255);
    rect(x, y, 200, 30);
    fill(255);
    textFont(fonts[1]);
    text("SerPort:", x + 5, y + 15);
  }
}

public void controlEvent(ControlEvent theEvent) {
  if (theEvent.isGroup())
    if (theEvent.name()=="portList") serPortManager.initPort(int(theEvent.group().value())); // initialize the serial port selected
}
